#include "kalman.h"

// 全局卡尔曼滤波器实例
KalmanFilter bat_kf; // 在卡尔曼滤波器库中定义

/**
 * @brief  初始化卡尔曼滤波器
 * @details 设置卡尔曼滤波器的初始参数，包括过程噪声、测量噪声和初始估计值，用于后续状态估计。
 * @param  kf: 指向卡尔曼滤波器结构体的指针
 * @param  q: 过程噪声协方差（反映模型不确定性，单位：无，建议值：0.001-0.1）
 * @param  r: 测量噪声协方差（反映传感器噪声，单位：无，建议值：0.01-1.0）
 * @param  initial_value: 初始估计值（例如电池电压的初始值，单位：V）
 * @return None
 */
void Kalman_Init(KalmanFilter *kf, float q, float r, float initial_value)
{
    kf->q = q;    // 过程噪声协方差
    kf->r = r;    // 测量噪声协方差
    kf->x = initial_value;  // 初始估计值
    kf->p = 1.0f; // 初始误差协方差
}

/**
 * @brief  更新卡尔曼滤波器状态
 * @details 根据新的测量值更新滤波器状态，结合预测和测量进行最优估计，输出平滑后的值。
 * @param  kf: 指向卡尔曼滤波器结构体的指针
 * @param  measurement: 当前测量值（例如电池电压，单位：V）
 * @return float: 滤波后的估计值（单位：V）
 */
float Kalman_Update(KalmanFilter *kf, float measurement)
{
    // 预测步骤
    kf->p = kf->p + kf->q;

    // 更新步骤
    kf->k = kf->p / (kf->p + kf->r);  // 卡尔曼增益
    kf->x = kf->x + kf->k * (measurement - kf->x);  // 更新估计值
    kf->p = (1 - kf->k) * kf->p;  // 更新误差协方差

    return kf->x;  // 返回滤波后的值
}
